within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model Test_DAE
  annotation (
    Diagram,
    experiment(
      StopTime=50,
      Interval=30000,
      Tolerance=1e-012),
    experimentSetupOutput,
    DymolaStoredErrors);
  parameter SI.Acceleration g=9.81;
  parameter SI.Length r1=0.5;
  parameter SI.Length r2=0.1;
  parameter SI.Length L=1.5;
  parameter SI.Mass md=0.5;
  parameter SI.Mass mr=0.5;
  parameter SI.Mass M=mr + 2*md;
  parameter SI.MomentOfInertia Idy=md*0.25*r1^2;
  parameter SI.MomentOfInertia Idz=md*0.5*r1^2;
  parameter SI.MomentOfInertia Irz=mr*0.5*r2^2;
  parameter SI.MomentOfInertia Iy=0.25*mr*(L^2/3 + r2^2) + 0.5*md*L^2 + 2*Idy;
  parameter Real phi0=0.1;
  parameter SI.AngularVelocity om0=0.3;
  parameter SI.Velocity[3] v0={-cos(phi0),0,sin(phi0)};
  parameter SI.Velocity[3] v10=v0 + cross({0,om0,0}, 0.5*L*{sin(phi0),0,cos(
      phi0)});
  parameter SI.Velocity[3] v20=v0 + cross({0,om0,0}, -0.5*L*{sin(phi0),0,cos(
      phi0)});
  parameter SI.AngularVelocity om10=-sqrt(v10*v10)/r1;
  parameter SI.AngularVelocity om20=-sqrt(v20*v20)/r1;
  parameter SI.AngularVelocity omz0=0;
  SI.Angle phi(start=phi0);
  SI.Angle phi1;
  SI.Angle phi2;
  SI.Force Fx1;
  SI.Force Fx2;
  SI.Force Fz;
  //  SI.Force Fz2;
  SI.Force N1;
  SI.Force N2;
  SI.AngularVelocity om(start=om0);
  SI.AngularVelocity om1(start=om10);
  SI.AngularVelocity om2(start=om20);
  SI.AngularVelocity omz(start=omz0);
  SI.AngularAcceleration eps;
  SI.AngularAcceleration eps1;
  SI.AngularAcceleration eps2;
  SI.Velocity v[3];
  SI.Velocity v1[3];
  SI.Velocity v2[3];
  SI.Position r[3](start={0,r1,0});
  SI.Position ksi;
  SI.Force B;
  SI.Torque M1;
equation
  der(r) = v;
  der(phi) = om;
  der(phi1) = om1;
  der(phi2) = om2;
  der(om) = eps;
  der(om1) = eps1;
  der(om2) = eps2;
  der(omz) = 0;
  M*r1^2*0.5/L*(om2^2 - om1^2) = Fz;
  -0.5*M*r1*(eps1 + eps2) = Fx1 + Fx2;
  Idz*eps1 = r1*Fx1;
  Idz*eps2 = r1*Fx2;
  Iy*eps = 0.5*L*(Fx1 - Fx2) - 0.3*sin(time);
  Irz*om*omz = 0.5*L*(N2 - N1) + M1;
  B = md*v1*v1/ksi + md*v2*v2/(ksi + L) + mr*v*v/(ksi + 0.5*L);
  Idz*(om1 + om2)*om = -r1*B - M1;
  0 = N1 + N2 - mr*g;
  ksi = om1*L/(om2 - om1);
  om*L = r1*(om2 - om1);
  v = -0.5*r1*(om1 + om2)*{cos(phi),0,-sin(phi)};
  v1 = (-0.5*r1*(om1 + om2) + 0.5*L*om)*{cos(phi),0,-sin(phi)};
  v2 = (-0.5*r1*(om1 + om2) - 0.5*L*om)*{cos(phi),0,-sin(phi)};

end Test_DAE;
